#include<iostream>
#include<KalmanCalculate.hpp>
#include<Armor.hpp>
#include<opencv2/opencv.hpp>

using namespace std;


class KalmanTracker{
public:
    // Armor
    Armor frameArmor;
    int frameNumber; // 屏幕上出现的装甲板编号 0-11
    int frameLostCount; // 屏幕上出现的装甲板消失时间
    Mat p_rvec,p_tvec;
    double distance;
    bool isFirst; //
    KalmanCalculate* klc; // 
    int id; // 装甲板固定号码

    // 带参构造
    KalmanTracker(const Armor &armor):
        frameArmor(armor),
        frameNumber(-1),
        frameLostCount(3),
        p_rvec(armor.rvec),
        p_tvec(armor.tvec),
        distance(armor.armorDistance),
        isFirst(true),
        klc(nullptr)
        {  };

    // 拷贝构造
    KalmanTracker(const KalmanTracker& other){
        if (other.klc != nullptr) {
            this->klc = new KalmanCalculate(*other.klc);  // 创建新的副本（这里klc缺少深拷贝构造）
        } else {
            this->klc = nullptr;
        }
        // 先对Armor进行基础拷贝
        this->frameArmor = other.frameArmor;
        if(!this->frameArmor.rvec.empty()){
            this->frameArmor.rvec = other.frameArmor.rvec.clone();
        }
        if(!this->frameArmor.tvec.empty()){
            this->frameArmor.tvec = other.frameArmor.tvec.clone();
        }
        if(!this->frameArmor.roiImg.empty()){
            this->frameArmor.roiImg = other.frameArmor.roiImg.clone();
        }
        // 基础拷贝
        this->frameNumber = other.frameNumber;
        this->frameLostCount = other.frameLostCount;
        this->p_rvec = other.p_rvec.clone();
        this->p_tvec = other.p_tvec.clone();
        this->distance = other.distance;
        this->isFirst = other.isFirst;
    }

    // 析构函数
    ~KalmanTracker()
    {  
        if(klc!= nullptr){
            delete klc;
            klc = nullptr;
        }
    };
    
    // 卡尔曼更新累计
    void CumulateKalman();

    // 初始化卡尔曼
    void inital_KalmanTracker(const Mat &rvec,const Mat &tvec);

    // 更新操作
    void update_KalmanTracker(const KalmanTracker &tracker);

};